#include "map.h"

void Map::Init() {
  for (int i = 0; i < dimension_x_; ++i) {
    std::vector<Node> row;
    for (int j = 0; j < dimension_y_; ++j) {
      row.push_back(Node(Point(i, j)));
    }
    nodes_.push_back(row);
  }
  cv::Mat background(dimension_x_ * kMapGridSize, dimension_y_ * kMapGridSize,
                     CV_8UC3, cv::Scalar(255, 255, 255));
  background_ = background;
}

Map::Map(cv::Mat image)
{
  auto cols = image.cols;//宽/列 x
  auto rows = image.rows;//高/行 y
  dimension_x_ = rows;
  dimension_y_ = cols;


  // for (int y = 0; y < image.rows; ++y) {
  //     for (int x = 0; x < image.cols; ++x) {
  //         cv::Vec3b pixel = image.at<cv::Vec3b>(y, x);
  //         // std::cout << "Pixel at (" << x << ", " << y << "): "
  //         //           << "B = " << (int)pixel[0] << ", "
  //         //           << "G = " << (int)pixel[1] << ", "
  //         //           << "R = " << (int)pixel[2] << std::endl;
  //         std::cout << static_cast<int>(pixel[0]) << " ";
  //     }
  //     std::cout << std::endl;  // 每行结束后换行
  // }
  // std::cout << std::endl;  // 每行结束后换行


  for (int i = 0; i < dimension_x_; ++i) {
    std::vector<Node> row;
    for (int j = 0; j < dimension_y_; ++j) {
      // cv::Vec3b pixel = image.at<cv::Vec3b>(j, i);
      cv::Vec3b pixel = image.at<cv::Vec3b>(i, j);
      int cost = 255 - static_cast<int>(pixel[0]);
      auto node = Node(Point(i, j));
      if(cost > 200)
      {
        node.set_is_obstacle(true);
      }
      else
      {
        node.set_is_obstacle(false);
      }
      row.push_back(node);
    }
    nodes_.push_back(row);
  }

  // for (int i = 0; i < rows; ++i) {
  //     for (int j = 0; j < cols; ++j) {
  //         std::cout << static_cast<int>(nodes_[i][j].is_obstacle()) << "  ";
  //     }
  //     std::cout << std::endl;  // 每行结束后换行
  // }

  cv::Mat background(dimension_y_ * kMapGridSize, dimension_x_ * kMapGridSize,
                     CV_8UC3, cv::Scalar(255, 255, 255));
  background_ = background;

  for (int i = 0; i < dimension_x_; ++i) {
    for (int j = 0; j < dimension_y_; ++j) {
      if (nodes_[i][j].is_obstacle()) DrawNode(nodes_[i][j]);
    }
  }

  cv::imshow("a_star", background_);
  // cv::waitKey(50000);

}

void Map::AddObstacle(const Obstacle& obstacle) {
  for (int i = 0; i < obstacle.dimension_x_; ++i) {
    for (int j = 0; j < obstacle.dimension_y_; ++j) {
      nodes_[obstacle.upper_left_x_ + i][obstacle.upper_left_y_ + j]
          .set_is_obstacle();
    }
  }
}

void Map::DrawObstacles() {
  for (int i = 0; i < dimension_x_; ++i) {
    for (int j = 0; j < dimension_y_; ++j) {
      if (nodes_[i][j].is_obstacle()) DrawNode(nodes_[i][j]);
    }
  }
}

void Map::DrawNode(Node node, cv::Scalar color,
                   int thickness) {
  int32_t x = node.point().x, y = node.point().y;
  cv::rectangle(background_, cv::Point(x * kMapGridSize, y * kMapGridSize),
                cv::Point((x + 1) * kMapGridSize, (y + 1) * kMapGridSize),
                color, thickness);
}

// void Map::DrawNode(Node node, cv::Scalar color,
//                    int thickness) {
//   int32_t x = node.point().x, y = node.point().y;
//   cv::circle(background_, cv::Point(x, y),
//                 0,
//                 color, -1);
// }
